#include <stdio.h>
#include <string.h>
#include "driver/ledc.h"
#include "esp_err.h"
#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "_74hc165_button.h"         // 74hc165按钮驱动
#include "bridge_motors.h"           // 电机驱动
#include "io_led.h"                  // 指示灯驱动

struct state_machine_data_i;
struct io_lights  lights;
struct bridge_motors  motor;
struct bridge_motors_config bm_cfg = {
    //(2<<13)/2   当前位宽下50%的计算方法
    .pwm_io1 = 1,
    .pwm_io2 = 3,
    .pwm_hz = 4000,
    .pwm_duty = ((2<<12)/2),
    .pwm_min_duty = 100,
    .pwm_max_duty = ((2<<12))-500,
    .pwm_add_duty = 1000,
    .pwm_sub_duty = 1000,
    .io_sd1 = 2,
    .io_sd2 = 4,
};
static int state_button_change_callback(struct state_machine_data_i *self);
/*判断用的按钮键值*/
enum button_reg 
{
    start_purse_button  = (1<<1),
    quick_button        = (1<<2),
    slow_button         = (1<<3),
    reversal_button     = (1<<4),
};

/*设备运行状态机*/
enum device_state
{
    device_state_ready_ok, // 准备就绪
    device_state_start_pause,    // 开始
    device_state_quick,    // 快速
    device_state_slow,     // 慢速
    device_state_reversal, // 反转
};

typedef int (*state_button_fn_t)(void *self);
typedef int (*motor_device_fn_t)(void *self);
struct state_machine_data_i
{
    enum button_reg button_operation;
    enum device_state state;
    struct _74hc165_button button;
    state_button_fn_t state_button_change;
    bool start_flag;                     // 区分启动和停止
    bool direction_flag;                 // 方向标志
};

int state_ready_ok(struct state_machine_data_i *self)
{
    self->state_button_change(self);
    return 0;
}

int state_start_pause(struct state_machine_data_i *self)
{
    if (self->start_flag == 1)
    {
        self->state_button_change(self);
        motors_correct(&motor);       
        printf("start\n");
        self->start_flag = 0;
        self->direction_flag = 1;
        io_lights_start(&lights);
        self->state = device_state_ready_ok;
    }else if(self->start_flag == 0){
        self->state_button_change(self);
        motors_stop(&motor);
        bm_cfg.pwm_duty = ((2<<12)/2);
        printf("pause\n");
        self->start_flag = 1;
        io_lights_stop(&lights);
        self->state = device_state_ready_ok;
    }
    return 0;
}

int state_quick(struct state_machine_data_i *self)
{
    if (self->start_flag == 0)
    {
        self->state_button_change(self);
        motors_hasten(&motor);
        printf("quick\n");
    }
    self->state = device_state_ready_ok;
    return 0;
}

int state_slow(struct state_machine_data_i *self)
{
    if (self->start_flag == 0)
    {
        self->state_button_change(self);
        motors_decelerate(&motor);
        printf("slow\n"); 
    }
    self->state = device_state_ready_ok;
    return 0;
}

int state_reversal(struct state_machine_data_i *self)
{
    self->start_flag = 0;
    self->state_button_change(self);
    if (self->direction_flag == 1)
    {   
        motors_inverse(&motor);
        self->direction_flag = 0;
    }else{
        motors_correct(&motor);
        self->direction_flag = 1;
    }
    printf("reversal\n"); 
    self->state = device_state_ready_ok; 
    return 0;
}

int state_machine_init(struct state_machine_data_i *self)
{
    struct _74hc165_config cfg = {
        .trigger_mode = 0,          //1立即触发，0 松开触发
    };

    self->start_flag = 1;              // 区分启动和暂停
    self->state_button_change = (state_button_fn_t)state_button_change_callback;  // 按钮回调
    io_lights_init(&lights);
    _74hc165_button_init(&self->button,&cfg);
    bridge_motors_init(&motor,&bm_cfg);
    self->state = device_state_ready_ok;
    return 0;
}

int state_machine_step(struct state_machine_data_i *self)
{
    vTaskDelay(10/portTICK_PERIOD_MS);
    switch (self->state)
    {
    case device_state_ready_ok:
        return state_ready_ok(self);
    case device_state_start_pause:
        return state_start_pause(self);
    case device_state_quick:
        return state_quick(self);
    case device_state_slow:
        return state_slow(self);
    case device_state_reversal:
        return state_reversal(self);
    }
    return 0;
}

void app_main(void)
{
    struct state_machine_data_i state_machine;
    state_machine_init(&state_machine);
    while(1)
    {
        state_machine_step(&state_machine);
    }
}

static int state_button_change_callback(struct state_machine_data_i *self)
{
    vTaskDelay(10/portTICK_PERIOD_MS);
    self->button_operation = _74hc165_button_get_num(&self->button);
    switch (self->button_operation)
    {
    case start_purse_button:
        self->state = device_state_start_pause;
        break;
    case quick_button:
        self->state = device_state_quick;
        break;
    case slow_button:
        self->state = device_state_slow;
        break;
    case reversal_button:
        self->state = device_state_reversal;
        break;
    }
    return 0;
}

